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ABSTRACT 


A general method of generating optimal trajectories between an 
initial and a final position of an n degree of freedom manipulator arm 
with^.nonl inear equations of motion is proposed. The method is based on 
the assumption that the time history of each of the coordinates can be 
expanded in a series of simple time functions. By searching over the 
coefficients of the terms in the expansion, trajectories which minimize 
the value of a given cost function can be obtained. 

The method has been applied to a planar three degree of freedom 
arm. The coordinates of the arm are the three joint angles. Two types 
of trajectories have been assumed. These are such that the time history 
for each joint angle is: 

1. a series expansion of polynomials, 

2. a series expansion of periodic functions. 

Two integral type cost functions have been used: 

1. the integral of the kinetic energy of the arm, 

2. the integral of the magnitude of the joint torques. 

The optimal values of the coefficients in the series expansion 
show a distinct pattern. For a particular combination of type of 
trajectory and cost function the optimal values of the coefficients have 
been approximated by rather simple functions. This results in suboptimal 
values of the coefficients, but they can be obtained without performing an 
on-line search. The difference between the optimal and suboptimal value 
of the cost function is of the order of 8%. 

Thesis Supervisor: Daniel E. Whitney 

Title : Associate Professor of Mechanical Engineering 
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CHAPTER 1 


INTRODUCTION AND PROBLEM STATEMENT 


The design and control of mechanical manipulators which perform 
functions similar to those of the human has been the subject of many 
recent, studies . A particular area of interest in these studies is the 
supervisory controlled manipulator. In supervisory control the operator 
specif ies task subgoals to a remote computer which in turn executes 
pieces of the task through direct command of the manipulator supported 
by local control loops. Visual sensors enable the operator to monitor 
the execution of the task. This technique is well-suited for a 
manipulator in outer space or other remote locations where the distance 
between the operator and the arm causes a significant time delay in the 
communication. Supervisory control can be applied equally well to 
performance of complex non-routine manipulation tasks as the routine 
execution of repetitive operations. Often it is required that a task 
is executed optimally in the sense that a particular cost function, for 
instance time or the expenditure of energy is minimized. 

The dynamic equations of motion of a manipulator arm are non- 
linear and would require nonlinear control techniques to minimize a 
given cost function. These techniques may require a considerable amount 
of computer storage and real time computation. Townsend [1] investi- 
gated the possibility of controlling a nonlinear arm with feedback 
control computed for the linearized arm motion equations. The non- 
linear equations of the arm are linearized about a certain desired motion. 
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It Is assumed that the deviations of the actual motion from the desired 
motion are small so that linear control lavs can be used to let the 
arm follow the desired motion. Townsend used two types of linear 
system controls: a regulator and a variable gain tracking technique. If 
the desired motion must be optimal the problem of how to generate the 
optimal motion strategies arises. This thesis describes a possible 
solution to this problem for a particular class of tasks, namely moving 
a manipulator arm from one position to another. 

If the manipulator arm has n degrees of freedom the position 
of the arm with n general coordinates Is described by a 

vector X In the coordinate space. The motion of the arm between an 
Initial position . . . . ,x^^] and a final position 

— f * ^^If *^nf^ described by a trajectory In the coordinate space 

between x^ and x^ . The motion Is optimal If the value of the cost 
function Is minimum along the trajectory. 
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CHAPTER 2 

METHOD OF GENERATING OPTIMAL TRAJECTORIES 


In this chapter a method of generating the optimal trajectory 
between an initial and a final position of a n degree of freedom 
mechanical arm is described. 


-2.1 General Approach 

The method is based on the assumption that the time history of 
each of the elements of the position vector x^(k =. 1» ..i, n) between 
and can be expanded in a series of simple time functions. 




( 2 . 1 ) 


+ + a, f (t) 

km m 


where * 0»...»m) are coefficients independent of time and A^ is 

constant. If ,f^(t) are given time functions Xj^ is only 

a function of the coefficients a. a, . The cost function J 

kl’ ’km 

which must be minimized along the trajectory between the Initial and 
final position of the arm is generally a function of » ^ts derivatives 
Xj^ and Xj^ , and the task performance time T . J takes the general 


form: 


J *= / L(x, X, x)dt 


( 2 . 2 ) 
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Substituting the functions for Xj^, and x^ in the expression for J , 
J can be written as a function of the parameters “ l****»n; 

£ * 0 m) and the performance time T . For a given T , J is only 


a function of the a's . So: 


(2.3) 


where a is the matrix of the parameters *“ 1» •••»«; £ = 0,...,m). 

By this procedure the problem of finding the optimal trajectory 
has been reduced to a parameter optimization problem, i.e. finding the 
values of the parameters a^^^^ for which the value of the cost function 
J(a^) is minimum. 

To obtain the optimal values of the parameters, one can follow 
different procedures which can be divided into two main categories: 

a. analytical method, 

b . numerical methods . 

The two methods are discussed briefly in the following sections. 


2.2 Analytical Method 


If there are no constraints on the possible values of the 
parameters and the function J(a^) has first and second partial 

derivatives everywhere, necessary conditions for a minimum are: 


0 


(2.4) 
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where is the i-th element of a with i “ £n + k and: 




9a^3aj 


(2.5) 


\^ich means that the matrix whose components are must 

be positive semidefinlte. Equation (2.4) will give as many equations as 
there are unknown parameters. The advantage of the analytical method 
is that it gives all the possible solutions. However, in practice 
this method can present problems if the function J(a^) is complicated. 


2.3 Numerical Methods 


There are various numerical methods available. Bryson and Ho 
[2], Bekey [3], Sage and Melsa [4] give a survey and a description of 
several of these methods. In general they are based on the following 
principle. Make an initial guess for the values of the parameters and 
supply these values as part of the input to a computer program. The 
program changes the values of the parameters according to a certain 
algorithm* until it has found a set of values for the parameters which 
minimizes the value of the cost function. The particular numerical 
method one uses depends on the behavior of the function J(a) as a 
function of its argument ^ . A disadvantage of these numerical methods 
is that , if d(^) has several local minima, only one local minimum is 
found, depending on the initial guess for the values of the a’s. This 
local minimum is not necessarily the global minimxim. The numerical 
methods are very suitable for the cases that d(a) is a complicated 


function. 
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CHAPTER 3 

GENERATION OF OPTIMAL TRAJECTORIES 
FOR A PARTICULAR MANIPULATOR ARM 


The generation of optimal trajectories as described In Chapter 
2 has been applied to a planar three degree of freedom mechanical arm. 

3.1 Description of the Manipulator Arm 

The arm consists of two rigid straight links with lengths 
and ^2 connected to a fixed reference frame and to each other by 
moveable joints. As shown In Fig. 3.1 the joint with the fixed frame 
Is considered as a double hinge with two degrees of freedom; the joint 
between the two links Is a hinge with one degree of freedom. The mass 
of the arm is lumped as two point masses m^ and m^ at the ends of 
the links. The point masses have no rotational moment of inertia about 
the axis of the associated links. This lumping of masses simplifies' 
the mathematics but does not affect the generality of the results. 

The following joint angles have been chosen as coordinates of 
the arm (see also Fig. 3.2): 

1 « the angle between the plane through the two links 

and a fixed plane through axis 1 , 

2 . the angle 62 between link 1 and a line in the plane of 
the arm perpendicular to axis 1 , 

3. the relative angle 6 ^ between link 1 and link 2. 


/ 





axis 
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- .1 - 


axis 1 



Figure 3.2. Coordinates of the manipulator arm. 

The arrows in Fig." 3.2 indicate the positive direction of rotation. 

This coordinate system is convenient both mathematically and physically 
for manipulators with torque drive at the joints. 

The position of the manipulator arm is described by a vector ^ 
with elements 6^^, 02t and 9^ . This 6^ corresponds to x used 

in Chapter 2. 

3.2 Trajectories 

Trajectories can be categorized depending on the type of functions 
f^(t)(i = 0,...,m) used in the expansion of the functions ” 1,2,3): 


A, + 










(3.1) 



For the purpose of this study two types of trajectories have been 
assumed. 

Type 1: 


The function for each joint angle 0^^ (k = 1,2,3) between 
t = 0 and t = T is a series expansion of polynomials. 


fo(t) = 


(3.2a) 


f^(t) 


t(T-t) 


(3.2b) 


f2(t) 


^ t( I - t)(T 
T 


- t) 


(3.2c) 


Only the first three terms of the series have been taken into account, 
so m = 2. 


For t < 0 and t > T the value of 0^ is equal to the function 
value at t=0 and t=T respectively. Figure 3.3. shows a plot of the 
functions their first and second derivatives. 

From the end conditions at t=0 and t=T , i.e. 


follows: 


and 








^kO ®kf ®ki 


(3.3a) 

(3.3b) 


(3.4) 


(3.5) 




Figure 3«3» Functl 
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The coefficients aj^j^ and are the ones to be chosen optimally. 


k2 


The expression for each 0j^ becomes: 


®ki ''' ^®kf * ®kl^ T ®kl ^2 


^2 ^3 2 ■ 


(3.6) 


This rather simple function does not provide a smooth start up and slow 
down of the manipulator arm because of the impulse singularities in the 
second derivatives. 

Type 2: 

The function for each joint angle 9j^ (k « 1,2,3) is a series 
expansion of periodic functions of the following form: 




(3.7a) 


f^(t) 


Y (t - ^ sin 2wt) , 0 < t y 


2 - I (t - sin 2o)t) , I 1 t < T 


(3.7b) 






f,(t) =<; 2 - ^ (t - sin 4u)t) , rlt<^ (3.7c) 


4 - - 4 


+ Y sin 4ojt) , ^ < t < T 





where a) * 2 tt/T . Also in this expansion only the first three terms 
have been taken into account. 

The functions their derivatives are plotted in 

Fig. 3.4. From the conditions (3.3) follows: 


= e 


ki 


and 


"kO 


■ \f - » 


ki 


(3.4) 


(3.5) 


• •• 

As 9^ and 0^^ are zero at t=0 and t=T this type of trajectory 
will give a smooth start up and slow down of the arm. 


3.3 Cost Functions 

Two cost functions have been used to optimize the trajectory 
between the initial and final position of the arm. Both cost functions 
are integral type functions. 

Cost Function 1: 


The integral between t=0 and t=T of the kinetic energy of the 
manipulator arm: 

T 

J = J KE dt (3.8) 

o 

where KE = kinetic energy of the arm at time t . 

For the particular arm studied here the expression for the 
kinetic energy of the arm at time t is: 


KE = O.5[0^{(mj^ + m2)8-^ cos^02 + ®2^2 ^ 


+ 2 ^2 ^ ® 3 ^^ 


(3.9) 


CONT'D. 



ZL 


+ } 

+ 9^ Q ^{ 2m ^ ^2 2 m 2 £^^2 ®3^ 


(3.9) 


This expression is derived. in Appendix lA. 

Cost Function 2: 

The integral between t*0 and t*T of the sum of the magnitude 
of the joint torques: 

T 3 

J = J I \u^\ dt (3.10) 

o k*l 

where u^ . = external torque applied at the 

Jc-th axis of rotation. 

This cost function is closely related to the energy consumed. 

From the dynamic equations of motion: 


follows: 


where 


T 6 = u + c 

u = T £ - c (3.12) 

u = [u^, U2» u^] , the vector representing 

the external torques. 


T 


T 

T 

T 


11 

T 

12 

T 

13 

21 

T 

22 

T 

23 

31 

T 

32 

T 

33 


the matrix representing 


the moments of inertia. 
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\ 


\ 


For the arm studied here 


^12 “ ^13 “ ^21 “ ^31 “ 


^ = [Cj^, C 2 > c^] , the vector representing 
the torques due to the reaction forces 
to centripetal and coriolis forces. 

The equations for the elements of u become: 


- ■=! 


(3.13a) 


"2 ' T 22 «2 + '^23 ®3 - '2 

“3 “ "^32 ®2 ^33 ®3 " *^3 <3. 13c) 

The expressions for the elements of T and are given in Appendix IB. 
A computer program which generates the equations of. motion for a manipu- 
lator arm of a given configuration was available. 

In both cost functions the influence of gravity has been omitted 
for two reasons. First, the position of a manipulator arm with respect 
to gravity will differ from case to case. For any particular case it 
will not be difficult to incorporate the influence of gravity in the 
cost function. Second, if a manipulator is used in outer space the 
influence of gravity is absent. 

3.4 Computer Programs 

Using the trajectories and cost functions described in the 
previous sections three combinations of cost function and type of 
trajectory are possible* 



Combination I: 


Minimizing J = / KE dt assuming trajectories of Type 1 
o 

(9^ Is a series expansion of polynomials). 

Combination II: 

T 

Minimizing J = / KE dt assuming trajectories of Type 2 
o 

(0j^ is a series expansion of periodic functions). 

Combination III: 

T 3 

Minimizing J = J ^ |u. | dt. assuming trajectories of Type 2. 

o k-1 


T 3 

. A combination of J = J ^ lu, | dt with trajectories of Type 1 

o k-1 .. 

is not possible. The joint torques = 1,2,3) are functions of 9^ . 

For the trajectories of Type 1 0^^ is infinite at t=0 and t-T. 

To obtain the optimal values of the parameters a^(i-l, . . . ,6) 

for a particular initial and final position of the arm a fortran coded 

computer program has been written for each combination of trajectory and 

cost function. The programs consist of: 

1. main program, 

2. numerical search routine, 

3. subroutine to compute the value 
of the cost function J(a). 

The three parts of the programs are described next. 
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1. Main Program 

The main program reads the input data (lengths and masses of 
the arm, initial and final position, performance time, and initial 
guess for the values of the coefficients in the expansion), calls the 
search routine and prints out the final (optimal) values of the 
coefficients and the cost function. The main program is basically the 
same for combination I, II and III. 

2. Numerical Search Routine 

The search routine used in this study is called pattern search. 
Pattern search is a direct search routine for minimizing a function 
J(^) of several variables a ^ [a^, a 2 » ....]. The argument st is 
systematically varied until the minimum of J(a^) is obtained. The 
pattern search routine determines the sequence of values for ^ ; an 
Independent subroutine computes the functional values of J(a_). 

A detailed description of the pattern search routine is given 
by Hooke and Jeeves [5]. 

Figure 3.5 shows a flow diagram of the search procedure as given 
by Hooke and Jeeves. 

3. Subroutine to Compute the Value of the Cost Function 

This subroutine is called by pattern search after each change in 
the argument ^ . It simulates the trajectory of a given type for the 
value of a^ supplied by pattern search and computes the value of J(a^) 
along the trajectory. The integration is carried out with Simpson's 
Rule. The number of intervals between t=0 and t*T is twenty. This 



Evaluate function. at 
initial base point 


m 



stop 


Figure Descriptive flow diagram of pattern search. 


. Ehter 



Exit 


Figure 3.?b. Descriptive flow diagram for exploratory moves. This 
routine is carried out for each coordinate separately 
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subroutine is different for each of the combinations of trajectory and 
cost function* 

The programs are listed in Appendix II. 
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CHAPTER 4 

DESCRIPTION AND RESULTS OF THE PROGRAM RUNS 

This chapter gives a description of the program runs made, 
possible difficulties in the use of the search routine and an interpreta- 
tion of the results of the searches. All runs were made for an arm with 
i^l = = 0.3 m and = m 2 = 1 kg. 

4.1 Initial and Final Positions 



ANGLE 0^^ (ANGLE 0^^) 


Figure 4.1. Combinations of initial (final) angles. 




28 


The Initial and final positions of the arm were chosen In a 
certain region In 0-space at discrete points. The dots in Fig. 4.1 
indicate the values of 62^ and 0^^ or 02^ and 0^^ . For fixed 
values of and 0^^^ this will give 256 possible combinations of 

initial and final position for each of the combinations I, II and III. 

To limit the number of rtms a choice was made out of the 256 possible 
combinations. 

For most runs 0^^^ »> -0.78 and 0^^^ = 0.78 (angles in radians) 
were chosen. For combination III a number of runs were made with different 
values for 0^^^ and 0^^^ while keeping ®2i’ ®2f* ®31 and 0^^ constant 

4.2 Results of the Searches 

The results of the individual searches for 0^^^ = -0.78 and 
0^^ = 0.78 are given in Appendices III A, B and C. The following 
observations can be made concerning the results for combinations I, 

II and III. 

I. For all combinations of and ^ the values of 

^12* ®22 ®32 zero. When both 02^^ = 02£ 

and 0^£ “ 0j£ the value of a^^j^ is zero too. 

II. When 02^ = ©2£ and 0^^ “ 0^£ the optimal values of 
*^11* ®21’ ®22 ®32 zero. The optimal value of 

a^2 lies between 0.11 and 0.18. 

III. As for I the optimal values of 3j^2» ^22 ^32 

zero or very small in all cases, and is zero when 

both 02£ “ 0££ and “ 0^£ • 
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For the special cases that both 0^^ * 02£ and 0^^ » 6^^ the 
optimal values of the parameters and for the three combinations 

of trajectory and cost function are plotted as functions of 02 ^^ ® 02 £ “ ©2 
and 6^^ = “ ®3 “ A. 2, 4.3 and 4.4. 

The results of the searches for combination III with varying 


®11 

and 


are 

listed in Appendix III D. 

The optimal values of 

®21’ 

®22 

and 

^32 

are zero in all cases. 

The optimal values of 

®21' 

^31 

and 

J(a) 

are plotted as function 

of 0j^^ - 0j^^ in Figs. 4.5, 

4.6 

and 4, 

.7. 





Each search gives only a local optimum. Therefore one can not 
be sure that the optimum found is a global optimum. However by starting 
the search In a proper point based on physical considerations one can 
increase the probability that the global optimum will be fotmd. An 
example of choosing a wrong starting point is given next. 

For combination III with 0 ^ « [-0.78, -0.78, 1.57] and 
® [0.78, -0.78, 1.57] the initial values of the a*s were chosen all 
equal to zero. This resulted in a set of optimal values for the a*s 
of [0.0, 0.0, 0.27, 0.0, 0.0, 0.0] and a value of the cost function J 
of 0.935. Starting the search at [0.0, -0.168, 0.492, 0.0, 0.0, 0.0] 
resulted in a set of optimal values for the a's of [0.0, -0.219, -0.626, 
0.0, 0.0, 0.0] and a value of the cost function J of .0.630 which is 
much less than in the other case. The new starting point was the result 
of an interpolation between the results for the other combinations of 
02 and 0^ (see Fig. 4.4). This example indicates that the choice of 



COEFFICIENT a-3T COEFFICIENT 




Figure b.2. Optimal values of coefficients &21 ®31 combination 

I I'Jith 02j^=B2^=02 ®3i“®3f”®3* 

The symbols in the fig\ires indicate the value of 0^: 

X — X 03=0.0; o — o 03=0.78; A- -A 0 ^= 1 . f> 7 ; 0----0 03=2,36. 




JOINT ANGLE 9^ 

Figure 1|.3, Optimal values of coefficients a .21 and for ccmbinatiOTi 

II with ^2±~^2{~^2 ®3i"®3f~®3' 

The symbols in the figures Indicate the value of 0^: 

X — X 0.j=O,O; O — O 00=0,78; A — A 0,=1,57; o- a 0o=2,36. 






COEFFICIENT a,, COEFFICIENT a 




Figure li.U. Optimal values of coefficients a2i ^31 ccmibinatiOTi 
III with S2i“®2f~®2 ® 3 i°® 3 f~® 3 * 


tfoo‘ 




33 





Figure I4.5. Optimal values of coefficients a.22^ and and cost 

function J(a) as functicais of ^lf"®li for combination 

in vri-th 02i»®2f““*^'7^ 03i=03f=O.78. 




Figure 1 |. 6 . Optimal values of coefficients a 2 i and and cost 

functicffi J(a) as functions of 9]_f-9xi combinaticMi 
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Figure h- 7 . Optimal values of coefficients 323^ and and cost 

function J(a) as functions of combination 

III with 02^=-O.78, 92£-=-1«?7 and 0 ^j|^= 9 :^^=O. 73 . 
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the starting point for the search can be very important. Therefore, 
the starting points have been chosen carefully in accordance with the 
physics of the problem. 

4 . 3 Some Examples of Optimal Trajectories 

In Fig. 4.8 and 4.9 two examples of how the functions for 6^^, 
will look like for different combinations of trajectory 
type and cost functions, using the optimal values of the coefficients 
in the series expansion. The Roman numbers in the figures indicate the 
combination of trajectory and cost function as mentioned in Section 3.4. 
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TIME 



O 


a 

O 


o 



0.0 1.0 2.0 

TIME 


Figure 1|.3. Time histories for 9]^, 02 and 0^ for 

^=[-0,78,0.0,0.0] and ^=[0.78,0.0,0.0]. 
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0.0 1.0 2.0 



0.0 1.0 2.0 



Figiire lj.9. Time histories for 0]^, ©2 and 9^ for 

©^=[-0. 78,0.0,0.0] and ©^ = [0.78,-0.78,1.57]. 
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CHAPTER 5 

GENERALIZATION OF THE RESULTS 


This chapter discusses a method of generalizing the results of 
the individual searches for specific combinations of trajectory and 
cost function. This method will lead to suboptiraal values of the 
coefficients a^^^ . However, these values can be obtained by on-line 
computation in a short time. It is not necessary anymore to perform 
an on-line search. 

The method is worked out for combination III where 
T 3 

J ^ ! 1 1^1.1 and the trajectories are of Type 2. 

o k=*l 

The values of 9^^^ and 0^^^ are - 0.78 and 0.78 respectively. 

For combination III the optimal values of ^^2’ ^22 ^32 

are zero or at least very small. Therefore the subopt imal values of 

these parameters have been chosen zero for all combinations of and 

. To obtain general expressions for determining the values of 

and a^, four categories of combinations of 0. and 0- have 
IX 2i “1 “t 

been considered separately. The assumptions made and the results 
obtained for each of the categories are described in the following 
sections. 


5.1 0 


li 


-0.78, = 0.78, 0^^ = 02f 


and 0^. = 0^^ 
3i 3f 


For this case the optimal values of a^^ are zero, 
values of a^j^ and a^^ are plotted in Fig. 4.4. 


The optimal 
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It is assumed that for each value of 6^ the values of 
and as functions of 0^ can be approximated by parabolic 

functions of the form: 

^21 “ ^ <5.1) 

^31 " *’^®2 - + d (5.2) 

Using a linear regression technique the best fitting parabolas through 
the four values of a-, and a^., for a constant 0.^ were determined. 

The formula used for this purpose is derived in Appendix IV. This 

gives four expressions for a^^ of the form (5.1) and four for a^^ 

of the form (5.2). Consideration of the values of p, a, b, q, c and d as 

functions of 0^ indicated that a good approximation for a,b,q,c 

and d as a function of 0^ is a straight line and for p as a function 

of 0^ is a parabola. Fitting the curves through the points the 


following expressions were obtained: 

p = 0.4936 - 0.3079 + 0.0504 0^ (5.3) 
a = -0.3929 - 0.2002 6^ (5.4) 
b = -0.6628 + 0.2619 0^ (5.5) 
q = -0.9985 + 0.3903 0^ (5.6) 
c = -0.3370 - 0.1106 0^ (5.7) 
d = 1.5442 - 0.5772 0^ (5.8) 



COEFFICI0JT a-, COEIFFICIENT a, 
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i~j 

CJ 



1 < 



JOINT ANGLE 0^ 


Figure 5.1. Functions for suboptimal values of coefficients a. 2 -^ and for 
combination III with ®2i~®2f~®2 ^3i*^3f~^3* 






hi 


Substituting (5.3) through (5.8) in Eqs. (5.1) and (5.2) ^21 

can be written as functions of 02 = 02 ^^ = 02 f ®3 “ ® 3 i “ ® 3 f * 

In Figure 5.1 the functions for a 2 j^ and a^^^ have been plotted 
for four values of 0^ . For comparison the optimal values of a 2 j^ 
and a^j^ obtained from the searches have been plotted too. 

The value of the cost function J(a^) has been computed for 
the suboptlmal values of the parameters and for all parameters equal to 
zero. This showed that on the average the difference between the sub- 
optimal value and the optimal value of J(^) is 7.4% of the optimal 
value. The difference between the value of J(a^) for all a's equal to 
zero. and the optimal value is 27.5% of the optimal value. 

5.2 0j^^ = - 0.78, = 0.78, “ ®2f ®3i ^ ®3f 

For this category the expressions for a 2 j^ and a^j^ mentioned 
in 5.1 have been used to determine the suboptlmal values of a 2 j^ and 
a^j^ as follows. Setting 02 = © 2 ^ = 02 £ 63 “ ® 3 £ Eqs. (5.1) 

through (5.8) gives certain values for a 2 j^ and , say a^^ and 



Setting 

®2 = 

®2i 

= 02 £ and 0^ = 0^^ 

the values of 

a 2 j^ and 

®3i ^21 

and a ^2 

. Using 

the following expressions a 

reasonable 

fit to 

the data 

for 

®2i 

° ®2£ *31 ’• ®3f 

was obtained. 




®21 

= 

min{a2j^. 

^21> 


(5.9) 



^31 

* 

min{a^j^. 

“ 31 ^ + rla'j 

- 

(5.10) 

where 


r 

-a 

1.3(02 

0.4)^ + 0.6 


(5.11) 
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For the suboptlmal value of parameter the expression 


= - 0.20 sgn {2 cos + cosCO^^^ + 

- 2 cos 02 J - cos( 02 j + 03f)) 
gave satisfactory results. 

5.3 0^^ = -0.78, = 0.78, © 2 ^ ^ Q^f ®3i “ ®3f • 

For this category the subop timal values for . a 2 j^ and 
obtained from (again using Eqs. . (5.1) through (5.8)): 

S 21 “ min{all a 2 ^ for 83 = 83 ^ - 83 ^ 

and 02 between 02 ^^, and 02 ^} 

“31 ' “31 ®3 ■ ®3i " ®3f 

and ©2 between 02 ^ and 02 f^ 

The value for a^^ followed from (5.12). 


(5.12) 


a 31 were 


(5.13) 


(5.14) 


5.4 0^^ = -0.78, 0^^ 


0^78^21 ^ ®2f ®3i ^ ®3f • 


Satisfactory values for the cost function J(a) were obtained 
by choosing for the values of a 2 j^ and a 33 (using (5.1) through 
(5.8)): 

32 j^ = min{all a 2 j^ for 

02 between 02 ^ and 02 ^ and 


83 between 83 ^ 


and 83 ^ } 


(5.15) 
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^31 * tnaxiall for 



between 

CD 

and 

0^^ and 


CD 

between 

CD 

and 


(5.16) 


The value of was chosen from Eq. (5.12). 

For the categories described In Sections 5.2» 3.3 and 5.4 the 
values of the cost function d(4) lor all a’s equal to zero are on 
the average 70%: bigger than the optimal values. For the suboptimal 
values of the als the difference between the values of JCa) and the 
optimal values was on the average 10%. This justifies the use of 


suboptimal values for the parameters 


very well. 


5.5 Flow Diagram for Determining the Suboptimal Values of the 


Coefficients in the Series Expansion 


The results of the previous sections can be summarized in a 
flow diagrm as presented in Fig. 5.2. The flow diagram is for the 
cases that 6^^^ « -0.78, 0^^^ = 0.78, 6^^ and between -1.57 and 

0.78, and 6^^ and 6^^ between 2.36 and 0 . The flow diagram is 
easy to transform into a computer program. 

The generalization as described in this chapter can also be 
done for each of the combinations I and II. The main problem will be 






ail =0 


ail 


Ei: ail 


^ 1 * an 


£2:321 


©2 25 ^2jL “ ^2£* 


®3 = ®3 

Eo: a>oi 


r = 1. 3(02+0. l|)^+0.6 

a’2i = min (a2l'»®2l' ' ^ 

831 = min (331', 331") + r | 


63 = 831 = 83^ 


321 = ®21 ®2 ^®'*'Ween 821 and 02f) 

331 = max (all 331 for 82 between 821 and 02f) 


821 = min (all 821 for 82 between 821 and ©2^ 

and 83 between 831 and ?3f^ 
3',-, r max (all a^, for 0p between and 0p£ 

^ and 83 between 831 and 93£)| 


CONT'D 
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Figure ^.2. Flow diagram for determining the sub- 
optimal values of ttie parameters 
321 ®31 combination III with 

9^ = -0.78 and 0^ = 0.78, 
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CHAPTER 6 
CONCLUSIONS 


The proposed method of generating optimal trajectories worked 
successfully for a three degree of freedom mechanical arm. However, 
the method is general and it can be expected that it is also applicable 
to more complicated arm models. 

The consistency of the results with the physical understanding 
of the problem indicates that the pattern search routine was suitable 
for this problem. 

The results of the searches indicate that only the first two 

terms of the series expansion are important, except for joint angle 0^^ 

in combination II where the third term has a significant influence on 

the shape of the function for , • 

^ T 

The optimal value of the cost function J = / KE dt for 

o 

combination I is lower than for combination II. From this it can be 
concluded that in order to minimize the integral of the kinetic energy 
a series expansion of polynomials is more suitable than a series 
expansion of periodic functions. To minimize the integral of the 
torque magnitude only the series expansion of periodic functions is 
applicable. 

In the special cases that 02^^^ * 02f ®31 * ® 3 f optimal 

values of the parameters a 2 j^ and a^^ showed a certain pattern, 
especially for combination III. For this combination the values of 
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322 ^32 summarized by paraboloid functions with 

^2i * ^2f * ^2 ^31 ^3f * ^3 independent variables (see 

Fig. 5.1). The values of 322 ^32 ot)tained in this way are 

suboptimal. The difference between the suboptlmal and the optimal value 
of the cost function is on the average 7.4%. From the suboptimal 

values of 322 a ^2 case that ®2i ** ®2f ®3i * ®3f 

the suboptimal values of 322 3^2 in the other cases of 

combination III could be derived quite easily. The suboptimal value of 
the cost function in these cases is on the average 10% bigger than the 
optimal value, which is satisfactory. 

Using the algorithm for the suboptimal values the coefficients 
in the series expansion can be obtained on-line without search. This 
saves a significant amount of real time computation. 
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APPENDIX I 


A. Derivation of the expression for the kinetic energy of the arm. 

For a system of two point masses, what the arm essentially 
is, the kinetic energy is; 


1 2^1 2 
KE . 5 mjVi + 5 


(I.l) 


where and V 2 are the velocities of the masses mj^ and m 2 * 
Furthermore ( see figure I.l ): 


^ 10 j ^ '^102 


( 1 . 2 ) 


where v. „ = component of v- due to rotation about axis 1, 

J- 

v-Q = component of v*. due to rotation about axis 2. 
IB -t 

'2 ■ + ''262 ^''2e2’'293'=°““ 


(1.3) 


where V 2 q “ component of V 2 due to rotation about axis 1, 

V 20 “ component of V 2 due to rotation about axis 2, 

v,Q = component of v_ due to rotation about axis 3, 

ot = angle between v-_ and v-» . 

202 203 

The expressions for the components of Vj^ and V 2 are: 


10, 


10 , 


' 20 , 


2., 63COS62 


£i02 


{2.3COS02 + 2,2^®®(02^®3^ ^®1 


(I. A) 

(1.5) 

( 1 . 6 ) 


20 , 


2.^ + 22.32.2COS02 62 


( 1 . 7 ) 
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^20^ “ 

Substituting (1.2) through (1.8) in (I.l) gives the 
following expression for the kinetic energy: 

1 2 ■ 2 2 2' 2 
KE » im^[ip^cos^92 ^1®2^ 

+ jtn2[{£j^cose2 + &2CO8 ( 82 + 03 ) >^ 6 ^ 

+ >^3 + «-2 + 2 l^l^cosQ^ '^02 

+ + ■¥ 02 ^ 2 ® 3 ®° s*^i 

Substituting: 


+^-1 cos0_ 

cosa ® 

/2 2 ' 

in Eqn. (1.9) and changing the order of the terms results in: 
KE = + m2)A^cos^02 + m2^-2COS^ (02+63) 

+ 02^^®1 ™ 2^^1 ^ ™ 2^2 ^ 2 m 2 ^ 3 ^ 2 ‘^‘^ 8 ® 3 ^ 

+ e 3 { m 2 < t 2 > 

+ 0262{2m22’2 + 2m25'322‘^°®®3^1 


(i.d) 


(1.9) 


( 1 . 10 ) 


(3.9) 
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B. Expressions for the elements of T and c in Eqn. (3.13). 


i“2^2 ^ l^“l ^ ni2)il^cos202 


+ ^m2i'2‘^os2 (62+63) + 2m2i-j^2’2Cos62Cos(02+62) 

(1. 11) 

^22 ”*1^1 ^ "’2^1 ^ ™2^2 ^ 2m22.j^22*^°®®3 

(1.12) 

T23 - ra2^2 "‘2^1^2‘^°®®3 

(1.13) 

^32 " ^23 

(1.14) 

^33 “ ®2^2 

(1.15) 

. . 2 2 
Cl « 6j^62{(nij^ + ni2)£j^sin202 + m2£2sin2(02+02) 


+ 2m22-j^22®^"^®3‘*'2®2^ ^ 


. . 2 

+ 6j^62{2m22^^X-2Cos02sin(02+02) + nj2Jl2sln2(02+02) } 

(1.16) 

C2 “ - 6^{|(mj^ + m2)2^sin202 + |m2S-2sln2(02+62) 


+ m22-j^2'2®^"^®3'*'2®2^^ 


+ 0^{m22-j^S,2®^"®3^ 


• • 

+ 026^{2m2Jtj^22®^“®3^ 

(1.17) 

Cj = - 0^{im22>2sin2(02+62) + m2J<j^S-2‘^°®®2^^"^®2^®3^ ^ 


- 02{ni22’j^2.2sin0^} 

(1.18) 
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APPENDIX II 


FORTRAN CODED COMPUTER PROGRAMS 




n n n n n 
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N/lIN PUCGRAy 

FCR CCMPLTING THF OPTIMAL VALUPS OF THE COEFFICIENTS 

IN THE SERIES EXPANSfCN 

TCRC IS THE SUBROUTINE FOR COMPUTING THE COST FUNCTION 

TCINT IS THE VALUE OF THF COST FUNCTION 

REAL LI, L2, Ml, M2 
EXTERNAL TORC 
DIMENSION BA{9) 

CCMMCN TFrN,F4,FS,F6, T9, ANGl I , ANG IF, ANG2 I , ANG2F, 
1ANG3I,ANG3F 

C READ INPUT DAT* 

READ(3,2C)TF TN, L 1 , L2 , Ml , M2 
2G - FGRMAT(E1S.6/4E15.6I 
REACt5,371N 

37 FCRMATIII) 

READ(5,lG)f)ELIN,DLKIN 
READ 15, SI M 

5 FCBMAT (121 

WRITEt6,4» 

4 FGRMATI • IGPTIMAL PARAMETERS FOR MIN INT TORQUE') 
WBITE(6,25) 

25 FGRMATT/,' TF I N, L I , L? , Ml ,M2 ' , / > 
KRITE16,261TFIN,LI,L2,MI,M2 
2fc FCRMATf5E15.6I 

WRITE{6,33) DELIN,CLMIN 

38 FCRMATt/,' DELfN^ »,F15.6,' CLM IN=* , E15 . 6 ,/ ) 

F1=L1*^2 

F2=L2>*2 

F3=L1«L2 

F4=M1*FI 

F5=M2«Fl 

F6=M2*F3 

Tg=M2«F2 

NCSET=I 

C READ INITIAL AND FINAL POST I ON OF T^E ARM 

h R E A D ( 5 , 1 C ) ANGl T , ANG 2 T , ANG3 I , A N G 1 F , A^! G 2 F , A NG3 F 
1C FCRMATIE 15.6) 

C READ INITIAL GUESS FOR THE PARAMETERS 

READ (5,10 (8A( I), 1=1,6) 

W9 lT£(fe,35) 

55 FCRMAT(/, • =*** INITIAL AND FINAL ANGLES',/) 

W R I T c ( 6 , 3 6 ) ANG 1 I , ANG2 I , A NG 3 1 , A NG I F , ANG 2 F , A NG 3F 
36 FGRMAT(3E15.6/3EI5.6,/) 

OEL=CELIN 

CALL PATSH ( 8A , TO I NT , N , D EL , DLM IN, TORG J 
WRITE(6,40) 

AC FORMAT (/,' FINAL RESULTS',/) 

WRITE{6,50) (BAm ,1=1,6) 

5C FORMAT (6E15.6) 

WRITE(6,60) TOINT 
6C FORMAT (F 15. 6) 
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IF (KCSET.EQ.M) GO TO 70 
NCSET=NCSET+1 
GC TC 6 
7C CCMINUE 
END 
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SUBRCUTIKE P AT SH ( PS 1 1 SS 1 1 N , D EL , CLM t MR [ T4 ) 

C PSI IS THE CURRENT BASEPT 
C THT IS THE PREVIOUS 8ASEPT 
C PHI IS THE TRIAL PT 
C S IS THE OBJECTIVE FCT 
C 

DIMENSION P5KR), PHI{9)t THT(9), E'»S(9) 
WRITE{6,603) 

6C3 FGRMATC CURRENT POINT, OBJ FCT AND STEPSIZE*) 
ALFA=1.02 

C EVALUATE AT INIT BASEPT 
1C CALL MRI TAIPSI ,SSI ) 

C START AT BASEPT 
ICO S=SSI 

DC ICl I=1,N 
IGl PHI(I)=PSI(I) 

ICALL=1 
WRITE (6, 599) 

599 FORMAT! • ♦♦♦• ) 

WRITE (6, 600) ( PSI { J) , J=1,N ) 

WRITE (6, 601) S, DEL 
C MAKE EXPLORATORY MOVES 
GO TO 150 

C IS PRESENT VALUE ) BASEPT VALUE 
160 IFIS.LT. SSI) GO TO 20C 
GO TO 300 
C SET NEW BASEPT 
2G0 SSI=S 

DC 2C1 l=l,N 
THT( I ) = PSI ( I) 

PSI(I) = PHim 
C MAKE PATTERN MOVE 

2 Cl PHI ( I )=PHI ( I )+ALFA«(PHI ( I)-THT( I ) ) 

CALL MRI TAIPHI ,SPI ) 

S=SPI 

WRITE(6,599) 

WRITE(6,599) 

WRITE(6,600) ( PHim, 1 = 1, N) 

6C0 FCRMAT(8E15.6) 

WRITE(6,601) S®I, DEL 
6C1 FGRMAT(2E15.6) 

ICALL=2 

C MAKE EXPL MOVES 
GO TO 150 

C IS PRESENT VALUE ) BASEPT VALUE 
260 IF(S.LT.SSI) GO TO 20C 
GO TO 100 

3C0 IFLDEL.LT. DLMIN) RETURN 
DEL=CEL/2. 

GO TO 100 
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C NAKe EXPL POVES 
150 DC 18C K=1,N 

EPS(K)=.C5*PHI (K) 

IF(EFS(K) .EQ. 0.) EPS(K)=.05 
PHI(K)=PHI (K)+FPS(K)*CEL 
CALL PRI TA (PHI tSPI ) 

155 IF(SPI.LT.S) GO TO 179 

PHI (K)=PHI (K)-?.*EPS(K)*DEL 
CALL PRI TA (PHI ,SP I ) 

165 IF(SPI.LT.S) GO TO 179 

PHI (K)=PHI (K)+EPS(K)*CEL 
GC TC 180 

179 S=SPI 

180 CONTINUE 

-GO TC ( 160,260 ) rICALL 
END 



non 
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SUBRCUTINE KNET( BA.KE INT ) 

FCR COMPUTING INTEGRAL OF THE KINETIC ENERGY 

WITH TRAJECTORIES OF TYPE I 

(SERIES EXPANSION OF POLYNOMIALS) 

REAL MALl,MAL2»MALA,KE(21 ) ,KFINT 
DIMENSION BA(9) 

CCMMCN TFIN.FA »F5,F6,T9, ANGl I , ANG IF, ANG2 I , ANG2F, 

IANG3 I ,ANG3F 
81=6A(1 ) 

B2=BA(2) 

B3=BA(3) 

B4=BA(4) 

B5=BA(5) 

B6=8A(6) 

TF2=TFIN*«2 

C COMPUTE KINETIC ENERGY AT EACH INTERVAL POINT 

DC 2G00 J=l,21 
T = TF IN«( FLOAT! J)-I. )/20. 

ANG2=ANG2I +T*( ANG2F-ANG2 I ) /T F IN+A.*R2*T« ( TF IN-T ) /TF2 
ANG2 = ANG2 + 64.»«=B5*T*(TF IN/2 .-T )’«' ( TF IN- T ) / ( 3 .♦ TF2* TF I N ) 
ANG3 = ANG3I+T*( ANG3F- ANG3 I ) /T F IN + A . 3<=T« ( TF I N-T ) /TF2 
ANG3-ANG3+6A.*B6«T*(TFIN/2.-T )«(TFIN-T)/(3.»TF2*TFIN) 
VELI=(ANGIF-ANG1I ) / TF IN+A. *B 1* ( TF IN- 2 .*T ) / TF 2 
VELI=VEL1 + 6A.*R4<'(0.5<‘TF2-3.*TFIN*T + 3.*T=M'2) / 
H3.*TF2*TF IN) 

VEL2=(ANG2F-ANG2I ) /TF IN+A . ♦B2« ( TF IN-2 . *T ) /TF 2 
VEL2=VEL2+6A.*B5*(0.5*TF2-3.«TFIN*T+3,*T*«2) / 

I (3.*TF2*TFIN) 

VEL3=(ANG3F-ANG3I ) /T F I N+A . *B3« ( T F I N- 2 .*T ) / TF 2 
VEL3 = VEL3 + 6A.*R6*(0.5*T F2-3.*TFIN=*T + 3.*T*»2) / 

1 (3.*TF2*TFIN) 

C SUM CF ANGLES 

FAC1-ANG2+ANG3 

C SINES ANC COSINES 

CCSl=COS ( ANG2) 

CGS2=CCS ( A\G3) 

CCSA=CGS (FACl) 

C COMBINE TERMS 

G1=FA+F5 

G6=F6«CDS2 

C COMPUTE 2*KINETIC ENERGY 

MALl=Gl«CnSl**2+T9*COSA**2+2 .*F6*C0S1*C0SA 

MAL2=Gl4^T9+2.«r,b 

MALA=2.* (T9+G6 ) 

KE (J ) = VEL1*=«'2*MAL1+VEL2**2*MAL2 + VEL3**2*T9 + 
1VEL2*VEL3*MALA 
2C00 CONTINUE 



C ccmplte integral of kinetic energy 

KEIM = KE (1 )+^.*KE(2)+KE(21 ) 

DC 2050 L=2,10 

2 050 KEIN r = KE INT + 2. *KE ( 2*L-l ) +4 .*K£ ( 2*L ) 
KEIM=ITFIN/60. )*KElNT/2. 

RETURN 
END . 



non 
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SUBRCUTIKE KINFT (BA,KEINT) 

FCR COMPUTING THE INTEGRAL OF THE KINETIC ENERGY 

WITH TRAJECTORIES OF TYPE 2 

(SERIES EXPANSION OF PERIODIC FUNCTIONS) 

REAL MALI ,MAL2 ,MAL^,KE(?1) ,KEINT 
DIMENSION BA(9) 

COMMON TF IN,F4,F5,F6, T9, ANGl I t ANG IF , ANG2 I , ANG2F , 
1ANG3I,ANG3F 
TCl = EA(l )«2. 

TC2=eA(2)«2. 

TC3=BA(3)*2. 

TC5=EA(5)*2. 

TC6=EA{6)*2. 

FC4=BA(4 
FC5=EA(S)*4. 

FC6 = EA{6 
OM = 6.283 I9/TFI N 
T0M=2.*0M 
FCM=4.*CM 

C COMPUTE KINETIC ENERGY AT EACH INTERVAL POINT 

DC 2C00 J=l,2l 
T=TFIN«(FLCAT( J)-l. )/20. 

OMT=CM«T 

TCMT=2.«CMT 

FGMT=4.*CMT 

SCMT=SINICMT) 

CCMT=CCS (GMT) 

STOMT=2. ♦SOMT^COMT 
CTGMT=l.-2.«SOWT**2 
SFCMT=2. *STOMT*CTGMT 
CFGMT = l.-2.«STOMT<‘*2 
PGNE=(T-SCMT/DM)/TFIN 
QGNE=( l.-COMT) /TFIN . 

PTWC^ (T-STGMT/T0M)/TFIN 
QTWO=(l.-CTOMT)/TFIN 
PFCU=(T-SFOMT/FOM)/TFIN 
QFGU=(l.-CFOMT I/TFIN 

C FIRST APPROX IMAT ION T BETWEEN 0 AND TFIN 

ANG2=ANG2I+( ANG2F-ANG2I )*PCNE 
ANG3=ANG3I+( ANG3F-ANG3 I )*PGNE 
VEL1=( ANGIF-ANGI I )«QONE 
VEL2=( ANG2F-ANG2I )*QONE 
VEL3=(ANG3F-ANG3I I^GONE 
IF(T-TFIN/2. ) 200,200,300 

C SECOND APPROXIMATION T BETWEEN 0 AMD TFIN/2 

200 ANG2=ANG2+TC2*PTWG 
ANG3=ANG3+TC3*PTWO 
VEL1=VEL1+TC1*QTWC 
VEL2 = \/EL2 + TC2*QTWO 
VEL3=VEL3+TC3*0TWG 
IF(T-TFI.N/4.) 600,600,700 
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C SECCND APPROXIMATION T BETWEEN TFIN/2 AND TFIN 

300 ANG2=ANG2+TC2-TC2*PTWC 
ANG3=ANG3+TC3-TC3«PTWC 
VEL1 = VEL 1-TC1*QTWC 
VEL2=VEL2-TC2*0TWG 
VEL3=VEL3-TC3*QTWG 
IF(T-.75=^TFIN) 700,70Ct8C0 

C THIRD APPROXIMATION T BETWEEN 0 AND TFIN/A 

600 ANG2=ANG2+FC5*PFOU 
ANG3 = ANG3 + FC6<=PFCU 
VEL1 = VEL 1 + FC4*QF0U 
VEL2=VEL2+FC5*CF0U 
VEL3=VEL 3+FC6*QFCU 
GC TC loco 

C THIRD APPRGXIMAT ION T BETWEEN TFIN/A AND 3*TFIN/A 

700 ANG2=ANG2+TC5-FC5«PF0U 
ANG3=ANG3+TC6-FC6*PFGU 
VELl=VELl-FCA*CFOU 
VEL2=VEL2-FC5*GFaU 
VEL3=VEL3-FC6*OFOU 
GG TC 1000 

C THIRD APPROXIMATION T BETWEEN 3*TFTN/A AND TFIN 

800 ANG2 = ANG2-FC5+FC^>*PF0U 
ANG3=ANG3-FC6+FC6«PF0U 
VEL1=VEL1+FC4*CF0U 
VEL2=VEL2+FC5«QFOU 
VEL3=VEL3+FC6*CF0U 

C :-SUM OF ANGLES 

ICOO FAC1=ANG2+ANG3 

C --SINES AND COSINES 

CCS1=C0S { ANG2) 

CCS2=CCS ( ANG3) 

CCS4=CCS (FACl) 

C — CCRBINE TERMS 
G1=FA+F5 
G6=F6*C0S2 

C COMPUTE 2*KINETIC ENERGY 

MALl=Gl«CCSl*«?+T9*CnSA**2 + 2.*F6*C0S l^COSA 
MAL2=Gi+T9+2 .*G6 
MIAL4=2.* (T9 + G6 ) 

KE { J )=VEL1**2«Mal 1+VEL2**2*MAL2 + VEL3*«2=5=T94 
1VEL2«VEL3<'MAL4 
2C00 CONTINUE 

C CCMPLTE integral OF KI.NFTIC ENERGY 

KEINT=KE (1 )+A.«KE(2)4KE( 21 ) 

DC 2C50 L=2,10 

2C50 KEINT = KE INT + 2.*KE(2*L-l ) +4.*KE( 2«L ) 

KEINT={TFIN/60. ) «K£ IN T/2. 

RETURN 

END 
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SUBRCUTINE TORQ (flfl.TCINT) 

FCR COMPUTING THE INTEGRAL OF THE SUM OF THE 

ABS. VALUES OF THE JOINT TORQUES 

WITH TRAJECTORIES OF TYPE 2 

(SERIES EXPANSION OF PERIODIC FUNCTIONS) 

DIMENSION BA(9),AU(21) 

COMMON TFIN,F4,F5,F6tT9,ANGlI,ANGlF,ANG2I,ANG2F, 
1ANG3I ,ANG3F 
TC1 = BA( I )^2. 

TC2=BA(2 )«2. 

TC3=BA(3 )*2. 

TC5=EA(5 )*2. 

TC6=EA(6)*2. 

FCA=EA (A 
FC5=EA(5)*A. 

FC6=BA(6 )*A. 

OM=6.28319/TFIN 
TOM=2.*OM 
FOM=A.*QM 

C COMPUTE JOINT TORQUES AT EACH INTERVAL POINT 

DC 2GOO J=l,21 
T = TF IN*{ FLOAT! J)-l. )/20. 

OMT=CM*T 
TCMT=2.*CMT 
FCMT=A.*CMT 
SOMT=SIN (OMT ) 

CCMT=COS (GMT ) 

STCMT=2.«SGMT*COMT 
CT0MT=1.-2.*S0MT«<‘2 
SF0MT = 2. =>STOMT*CTCMT 
CFGMT=1.-2.*ST0MT*»2 
PCNE= (T-SCMT/OM) /TF IN 
QGNE=(1.-CCMT)/TFIN 
RCNE=OM*SCMT/TFIN 
PTWC=( T-STOMT/TOM )/TFIN 
QTWG=(l.-CTOMT)/TFIN 
RTWG=TGM*STOMT/TF IN 
PFCU=(T-SFOMT/FOM)/TFIN 
OFGU=( l.-CFQMT )/TFIN 
RFCU=FOM*SFOMT/TF IN 

C FIRST APPROXIMATION T fiETWEEi'i G AND TFIN 

ANG2 = ANG2I + ( ANG2F-ANG2 I )*PONE 
ANG3=ANG3I+ ( ANG3F-ANG3I )*PGNE 
VEL1= (ANGIF-ANGI I )*QGNE 
VEL2=(ANG2F-ANG2I )*QONE 
VEL3=(ANG3F-ANG3I )*QPNE 
ACC1= ( ANGlF-ANGl I )*RCNE 
ACC2=(ANG2F-ANG2I M-RONE 

ACC3=^ ( ANG3F-ANGU )#RnNE 

IFIT-TFIN/2. ) 200,200,300 
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C SECCNC APPROXI MAT f GN T BETWEEN C AMD TFIN/2 

200 ANG2=ANG2+TC2«PrWG 
AKG3=ANG3+TC3*PTWG 
VELl^VELl+TCl^QTWG 
VEL2=VEL2+TC2*CTWG 
VEL3=VEL3+TC3*CTWC 
ACC1=ACC1 + TC1<'RTWC 
ACC2-ACC2+TC2«RTWC 
ACC3 = ACC3 + TC3<'RTWG 
IF(T-TFIN/A. ) 600,600,700 

C SECOND APPRQXIMAT ICN T BETWEEN TFIM/2 AND TFIN 

300 ANG2=ANG2+TC2-TC2«PTWC 
ANG3 = ANG3+TC3-TC3=»PTWG 
VELl=VELl-TCl*CTwG 
VEL2=VEL2-TC2*QTWG 
VEL3=VEL3-TC3*0IWC 
ACC1=ACC1-TCI*RTW0 
ACC2=ACC2-TC2*RTWC 
ACC3=ACC3-TC3*RTWG 
IF (T-.75«TFIN) 700,700,800 

C THIRD APPROX IMAT I CN T BETWEEN 0 ANP TFIN/A 

600 ANG2=ANG2+FC5«PF0U 
ANG3=ANG3+FC6*PF0U 
VELl=V.ELl + FC4*CF0U 
VEL2=VEL2 + FC5<‘0F0U 
VEL3=VEL3+FC6*QF0U 
ACC1 = ACCUFC4*RF0U 
ACC2=ACC2+FC5*RF0U 
ACC3=ACC3+FC6*RFau 
GC TC lOCO 

C THIRD APPROXIMATION T BETWEEN TFIN/A AND 3*TFIN/A 

700 ANG2=ANG2+TC5-FC5*PFDU 
ANG3 = ANG3 + TC6-FC6=«'PF0U 
VEL1 = VEL l-FC4*OFOU 
VEL2=VEL2-FC5*OFOU 
VEL3=VEL3-FC6«QF0U 
ACCI=ACC1-FC4*RF0U 
ACC2=ACC2-FC5*RFUU 
ACC3=ACC3-FC6«RFDU 
GC TC locn 

C THIRD APPRCXIMATICN T BETWEEN 3=^TFTN/A AND TFIN 

800 ANG2 = ANG2-FC5 + FCB=!-PF0U 
ANG3=ANG3-FC6+FC6«PFCU 
VEL1=VEL 1+FCA«QF0U 
VEL2=VEL2+FC5*CF0U 
VEL3=VEL3+FC6*QF0U 
ACCI=ACC1+FCA*RF0U 
ACC2=ACC2+FC5*RFOU 
ACC3=ACC3+FC6*PF0U 
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C SUH CF ANGLES 

ICOO FAC1=ANG2+ANG3 
FAC2=2.*FAC1 
FAC3-2.+ANG2 
FAC4=ANG3+FAC3 

C SINES ANE COSINES 

SIN2=SIN (ANG3) 

CCSl=COS ( ANG2) 

CGS2=CGS ( ANG 3) 

SIN3=SIN (FAC3) 

SIN4=SIN(FAC1) 

SIN5=SIN (FAC2) 

SIN6=SIN IFACA) 

CGS3=CCS (FAC 3) 

ces4=ecs (FAci) 

CCS5=CCS (FAC2) 

e CGI* BINE TERMS 

G1=F4+F5 

G2=T9*SIN5 

G3 = F6*SIi\6 

G4=F6*C0S1 

G5=F6»SIN2 

G6-F6+CGS2 

G7=2 .♦G4«SIN4+G2 

G8 = G 1*S I N3 + G2 + 2 , *G3 

C CCMPUTE FRGOUCTS GF VELGCITIES 

Vl=. 5*VEL1*«2 

V?-VEL2*<=2 

V3=VEL3#«2 

V4=VEL1«VEL2 

V5=VEL1*VEL3 

V6 = VEL2<=VEL3 

C CGMPUTE C-TERMS 

Cl=V4*Ga+V5*G7 

C 2 = - V 1 *G 8 + ( V 3+ 2 , * V 6 ) * G 5 

C3=-V1*G7-V2*G5 

G COMPUTE T -TERMS 

T6-T9+G6 
T5=T9+2. *G6+G1 
T8 = T6 

T1 = .5*(G1#( 1+CGS3 )+T9*( 1+COS5) ) + 2.*G4*CQS4 

C COMPUTE JOINT TORCUtS 

UI = T l« AC C l-C l 

U2-T3«ACC2+T6*ACC3-C2 

U3-T8*ACC2+T9f=ACC3-C3 

C CCMPUTE SUV CF ABS. VALUES OF JOINT TGROUES 

AU ( J )=A0S(U1 )+ABS (U2 ) +ABS( U3 ) 

2 COO CONTINUE 



65 


C CCr'PUTE INTEGRAL OF AES. JOINT TCiRQUES 

TCINT = AU(l)+A.*AU(2)+AU{2l ) 

DO 2050 L=2,10 

2 050 TGINT = T0INT + 2.*AU(2«L-l)+4.*AU(2*L ) 
TGINT=(TFIN/60. I^TOINT 
RETURN 
END 
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APPENDIX III 


RESULTS OF THE SEARCHES 


Optimal values of the coefficients in the 
series expansion and the cost function J(a) 
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III A. 



“ 0*^ KE dt ; trajectories of type 1 (series expansion of polynomials) 
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III C. 
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III C cont'd. 
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III D. 
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APPENDIX IV 

DERIVATION OF THE LINEAR REGRESSION FORMULA 

If the result of an experiment is a number of data points 
( x^, ) and the model for the experiment is 

assumed: 


y ® Cq + c^x + C2X + • . . . + Cj^x 


K 


(IV. 1) 


one can write the following N equations for the estimates of 
the y’s: 


Cq + + C2X^ + 




+ Cj^X^ 




(IV. 2) 


or in matrix form: 


•• 

^ = X £ 


(IV. 3) 


where 


i = [ y^, 72. 5 

1 K 

1 X 




K 


^ ^ 2 ****** ^ 



-Ib- 


is'. 


The error between the actual estimate of it 


®. “ Z ~ 2i£. (IV. A) 

The squared error becomes: 

«^® “ ( Z - 2^ )’^( - 3te ) (IV. 5) 

The £ that will minimize the squared error is £ , the least square 
estimate of £ , and follows from the necessary condition for 
a minimum squared error: 


or:. 


So that: 


8£^£ 

“ 5 ^ 


T T 
X z + X & » 0 


£ M A rh\ 


(IV.6) 


(IV. 7) 


(IV. 8) 
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